/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date October 2025
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

// #ifndef Q_MOC_RUN
// #include "rclcpp/rclcpp.hpp"
// #include <std_msgs/msg/string.h>
// #endif
#include "qnode.hpp"

#include "rclcpp/rclcpp.hpp"
#include <string>

#include <sstream>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <QObject>
#include <QDebug>



/*****************************************************************************
** Implementation
*****************************************************************************/
using std::placeholders::_1;

QNode::QNode(int argc, char** argv ) :
    init_argc(argc),
    init_argv(argv)
{

}

QNode::~QNode()
{
    if(rclcpp::ok()) {
        rclcpp::shutdown(); // explicitly needed since we use ros::start();
    }
    wait();
}

bool QNode::init()
{
    rclcpp::init(init_argc, init_argv);
    nh = std::make_shared<rclcpp::Node>(QNODE_NAME);
    if (!rclcpp::ok()) {
        RCLCPP_INFO(nh->get_logger(),"QNode::init failed!");
        return false;
    }
    // Add your ros communications here.
    init_pubsub();
    init_param();
    start();
    RCLCPP_INFO(nh->get_logger(),"QNode inited...!");
    
    return true;
}

void QNode::run()
{
    std::cout << "Ros started, proceeding to run the gui." << std::endl;
    rclcpp::Rate loop_rate(10);
    while (rclcpp::ok()) {
        rclcpp::spin_some(nh);
        loop_rate.sleep();
    }
    std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
    Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}

int QNode::set_image_preview(bool flag)
{
    if (flag == img_preview) {
        return 0;
    }

    RCLCPP_INFO(nh->get_logger(), "Set image preview to %d", flag);
    if(flag) {
        // image_sub = nh->create_subscription<sensor_msgs::msg::Image>("/camera/color/image_raw", 5, std::bind(&QNode::imageCallback, this, std::placeholders::_1));
        img_preview = true;
    } else {
        img_preview = false;
        // image_sub.reset();
    }
    return 0;
}

int QNode::set_image_preview(ImgPreviewType type)
{
    if (type == img_preview_type) {
        return 0;
    }
    RCLCPP_INFO(nh->get_logger(), "Set image preview type to %d", (int)type);
    img_preview_type = type;
    return 0;
}

int QNode::get_share_directory(QString &path)
{
    path = share_directory;
    return 0;
}

int QNode::get_config_directory(QString &path)
{
    path = share_directory + "/config";
    return 0;
}

void QNode::init_pubsub()
{
    battery_sub = nh->create_subscription<std_msgs::msg::Float32>("/PowerVoltage", 5, std::bind(&QNode::batteryCallback, this, std::placeholders::_1));
    img_sub = nh->create_subscription<sensor_msgs::msg::Image>("/camera/color/image_raw", 5, std::bind(&QNode::image1_callback, this, std::placeholders::_1));
    // comp_img_sub = nh->create_subscription<sensor_msgs::msg::CompressedImage>("/camera/color/image_raw/compressed", 5, std::bind(&QNode::image2_callback, this, std::placeholders::_1));

    // Cmd_Vel_Sub = nh->create_subscription<nav_msgs::msg::Odometry>("/odom", 5,std::bind(&QNode::speedCallback, this, std::placeholders::_1));
    // sub_camera("/camera/color/image_raw/compressed");
    RCLCPP_INFO(nh->get_logger(), "pubsub topics done...!");
}

void QNode::init_param()
{
    share_directory = ament_index_cpp::get_package_share_directory(QNODE_NAME).c_str();
    nh->declare_parameter<int>("image_fps", 5);
    nh->get_parameter("image_fps", img_fps);
    img_show_interval = 1000 / img_fps;
    RCLCPP_INFO(nh->get_logger(), "image_fps: %d, img_show_interval: %.2f ms", img_fps, img_show_interval);
}

void QNode::batteryCallback(const std_msgs::msg::Float32::SharedPtr message)
{
    RCLCPP_INFO(nh->get_logger(), "battery : %f", message->data);
    emit batteryState(message->data);
}

void QNode::image1_callback(const sensor_msgs::msg::Image::ConstSharedPtr msg)
{
    auto clock = std::make_shared<rclcpp::Clock>();
    auto time = clock->now();
    auto duration = (time - img_update_time).nanoseconds() / 1e6;

    if (!img_preview) {
        return;
    }

    if (duration < img_show_interval) {
        return;
    }
    // RCLCPP_INFO(nh->get_logger(), "image time : %.2f", time.seconds());

    img_update_time = time;

    cv_bridge::CvImageConstPtr cv_ptr;
    try {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    } catch(const std::exception & e) {
        RCLCPP_INFO(nh->get_logger(), "image subscription error!!!");
        return;
    }
    QImage im=Mat2QImage(cv_ptr->image);
    emit image_val(im);
}

void QNode::image2_callback(const sensor_msgs::msg::CompressedImage &msg)
{
    cv_bridge::CvImageConstPtr cv_ptr;
    try {
        cv_ptr=cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    } catch(const std::exception & e) {
        RCLCPP_INFO(nh->get_logger(), "image compress subscription error!!!");
        return;
    }
    QImage im = Mat2QImage(cv_ptr->image);
    emit image_val(im);
}

QImage QNode::Mat2QImage(cv::Mat const& src)
{
    QImage dest(src.cols, src.rows, QImage::Format_ARGB32);
    const float scale = 255.0;
    if (src.depth() == CV_8U) {
        if (src.channels() == 1) {
            for (int i = 0; i < src.rows; ++i) {
                for (int j = 0; j < src.cols; ++j) {
                    int level = src.at<quint8>(i, j);
                    dest.setPixel(j, i, qRgb(level, level, level));
                }
            }
        } else if (src.channels() == 3) {
            for (int i = 0; i < src.rows; ++i) {
                for (int j = 0; j < src.cols; ++j) {
                    cv::Vec3b bgr = src.at<cv::Vec3b>(i, j);
                    dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
                }
            }
        }
    } else if (src.depth() == CV_16U) {
        if (src.channels() == 1) {
            for (int i = 0; i < src.rows; ++i) {
                for (int j = 0; j < src.cols; ++j) {
                    int level = (int)src.at<ushort>(i, j);
                    dest.setPixel(j, i, qRgb(level, level, level));
                }
            }
        } else if (src.channels() == 3) {
            for (int i = 0; i < src.rows; ++i) {
                for (int j = 0; j < src.cols; ++j) {
                    cv::Vec3b bgr = src.at<cv::Vec3b>(i, j)*65535;
                    dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
                }
            }
        }
    } else if (src.depth() == CV_32F) {
        if (src.channels() == 1) {
            for (int i = 0; i < src.rows; ++i) {
                for (int j = 0; j < src.cols; ++j) {
                    int level = scale * src.at<float>(i, j);
                    dest.setPixel(j, i, qRgb(level, level, level));
                }
            }
        } else if (src.channels() == 3) {
            for (int i = 0; i < src.rows; ++i) {
                for (int j = 0; j < src.cols; ++j) {
                    cv::Vec3f bgr = scale * src.at<cv::Vec3f>(i, j);
                    dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
                }
            }
        }
    }

    return dest;
}
